package robot;

import javax.vecmath.Point2d;
import javax.vecmath.Vector3d;

import pathAlgorithms.AStar_DStarPathTime;
import pathAlgorithms.PathAlgorithm;
import program.Main;
import program.MovingObjects;
import program.RoboMap;

/**
 * This robot use DStar to find its way. It can navigate in a world of moving
 * objects. Every moving object is consider to only have constant speed and
 * direction.
 * 
 * @date 9 maj 2012
 * @version 1.0
 */
public class DStarDynRobot extends Robot {
	private PathAlgorithm pathfinder;
	private Point2d[] path;
	private Point2d goal;
	private int pathIndex;

	/**
	 * 
	 * @param startPosition
	 *            for the robot
	 * @param name
	 *            of the robot
	 * @param goal
	 *            final position
	 * @param mo
	 */
	public DStarDynRobot(Vector3d position, String name, Point2d goal,
			MovingObjects mo) {
		super(position, name, mo);
		this.pathfinder = new AStar_DStarPathTime();
		this.goal = goal;
		this.pathIndex = 0;
		setFinished(false);
	}
	
	public DStarDynRobot(Vector3d position, String name, PathAlgorithm pathFinder, Point2d goal,
			MovingObjects mo, int sensorRange)  {
		super(position, name, mo,sensorRange);
		this.pathfinder = pathFinder;
		this.goal = goal;
		this.pathIndex = 0;
		setFinished(false);
	}

	public DStarDynRobot(Vector3d position, String name, Point2d goal,
			MovingObjects mo, int sensorRange) {
		super(position, name, mo, sensorRange);
		this.pathfinder = new AStar_DStarPathTime();
		this.goal = goal;
		this.pathIndex = 0;
		setFinished(false);
	}

	@Override
	public void performBehavior() {
		if (!isFinished()) {
			// This is initBehavior()
			updateRoboMap();
			if (path == null) {
				 this.path = pathfinder.getPath(this.roboMap,
						 	 this.getCurrentPosition(), this.goal, getLifeTime());
				if(path != null) {
		            	roboLogger.pathsCalculated++;
	            }
				this.pathIndex = 0;
				if (path != null) {
					rotateTowardsNextGoal(path[pathIndex]);
					setTranslationalVelocity(1);
				} else {
					setFinished(true);
					setTranslationalVelocity(0);
					return;
				}
			}
			
	    	if( sensors.getMeasurement(0) < 0.4 || 
    			sensors.getMeasurement((int)(0.125*sensors.getNumSensors())) < 0.4 ||
	 	        sensors.getMeasurement((int)(0.875*sensors.getNumSensors())) < 0.4) {
		    		updateRoboMap();
		    		this.path = pathfinder.getPath(roboMap, getCurrentPosition(), goal, getLifeTime());
		    		if(path != null) {
		            	roboLogger.pathsCalculated++;
		            }
		    		this.pathIndex = 0;
		    	}
	    	
	    	boolean objectInPath = false;
			
	    	if(pathIndex < path.length-1) {
	    		objectInPath = objectConflict(getCurrentPosition(),
					path[pathIndex+1], getLifeTime());
	    	}
			if (Double.isInfinite(this.path[pathIndex].x)) {
				if (this.path[pathIndex].y > 1) {
					setTranslationalVelocity(0);
					this.path[pathIndex].setY(this.path[pathIndex].y - 1);
				} else {
					setTranslationalVelocity(1);
					pathIndex++;
				}

			} else if (objectInPath || this.getCurrentPosition().distance(this.path[pathIndex]) < ACCEPT_DISTANCE) {
				// reach a way point.
				roboLogger.path.add(path[getIndex()]);
				setTranslationalVelocity(0);
				this.pathIndex++;
				if (pathIndex < this.path.length && !Double.isInfinite(this.path[pathIndex].x)) {
					
					if (!objectInPath) {
						double distance = 0;
						for (int i = pathIndex; !objectInPath
								&& i < path.length - 1; i++) {
							distance += path[i].distance(path[i + 1]);
							objectInPath = objectConflict(path[i], path[i + 1],
									getLifeTime() + distance);
						}
					}
					if (objectInPath) {
						 path = pathfinder.getPath(roboMap,
								 getCurrentPosition(), goal, getLifeTime());
						 if(path != null) {
				            	roboLogger.pathsCalculated++;
			             }
						 pathIndex = 0;
					}
					if (path != null) {
						rotateTowardsNextGoal(this.path[pathIndex]);
						setTranslationalVelocity(1);
					}
				}
			}
			if (path == null || this.pathIndex >= this.path.length) {
				setFinished(true);
				roboLogger.distanceTraveled = getOdometer();
				setTranslationalVelocity(0);
				if(this.getCurrentPosition().distance(goal) >= ACCEPT_DISTANCE) {
					Main.finishRun(roboLogger, "DStarDynRobot, "+getName()+", No path to goal... :'(");
				}else {
					Main.finishRun(roboLogger, "DStarDynRobot, "+getName()+", Goal reached! :)");
				}
				return;
			}
		} else {
			if (this.getCurrentPosition().distance(goal) >= ACCEPT_DISTANCE) {
				rotateY(0.1f);
				return;
			} else {
				if (getCounter() % 50 == 0) {
				}
				return;
			}
		}
	}

	private boolean objectConflict(Point2d from, Point2d to, double time) {
		double t = 0.3;

		if (roboMap.contain(RoboMap.Content.OBJECT, to, time + t)) {
			return true;
		}

		if (from.x != to.x && from.y != to.y) {

			int x = from.x < to.x ? 1 : -1;
			int y = from.y < to.y ? 1 : -1;

			return roboMap.contain(RoboMap.Content.OBJECT, new Point2d(from.x
					+ x, from.y), time + t)
					|| roboMap.contain(RoboMap.Content.OBJECT, new Point2d(
							from.x, from.y + y), time + t);
		}
		return false;
	}

}
